The present invention relates to industrial robotic systems of the type including a robot device, which is typically six-jointed, and computer-driven control and displacement units connected for effecting rotation of each joint.
Many industrial assembly operations require that one part contact and then slide with respect to a second part until the plane of contact changes abruptly and the first part drops into place in the second part. This kind of task is easily accomplished if the one part is restricted to straight line motion with a constant force applied to this part in the direction of the straight line motion. This is an example of force/position control in which force is controlled in a given direction and position is controlled in a plane perpendicular to the force direction.
This type of control is simple to achieve with a cartesian coordinate robot when the straight line motion is aligned with one of the robot axes. However, with jointed robots, force/position control is more difficult since in general all six joints must be controlled simultaneously in order to effect straight line motion of the robot manipulator.
The basic components of a six-jointed robot are illustrated in simplified form in FIG. 1. This device includes a fixed base 2, five linking members 4, 6, 8, 10 and 12 and an end member 14 which carries or guides the part to be assembled and is normally provided with some type of gripping mechanism.
Base 2 and members 4, 6, 8, 10, 12 and 14 are coupled together by pivot joints 21, 22, 23, 24, 25 and 26 each having a joint motor. Pivot joints 22, 23 and 25 have transverse axes and joints 21, 24 and 26 have longitudinal axes.
Each member can thus be pivoted about a respective joint axis from a reference angular position, the angular deviation from the reference position being known as the joint angle for that joint. The joint angles for joints 21-26 are designated .theta..sub.1, .theta..sub.2, .theta..sub.3, .theta..sub.4, .theta..sub.5 and .theta..sub.6.
The precise position and orientation of member 14 is a function of the six joint angles. The position of member 14 can be defined in terms of the x, y and z coordinates of a reference point on member 14 relative to a fixed coordinate system. The orientation of member 14 can be defined by the orientation of a selected plane of member 14 containing the reference point and in terms of three Euler angles .phi., .theta. and .PSI. which designate the orientation of the selected plane relative to the fixed coordinate system. These angles are designated on the basis of the orientation of a local coordinate system associated with the selected plane relative to the fixed coordinate system, as will be explained with reference to FIGS. 2A-2D in which the reference point on member 14 is designated R.
In FIG. 2A, a fixed plane 52 parallel to the x-y plane of the fixed coordinate system is congruent with the selected plane 54 of member 14. Plane 54 is associated with coordinates x', y', z' congruent with fixed coordinates x, y, z, respectively. Plane 54 can have any orientation relative to plane 52, defined by angles .phi., .theta. and .PSI.. If, to arrive at the final orientation shown in FIG. 2D, plane 54 is rotated about congruent axes z, z', an angle .phi. is formed between axes x and x'. Then rotation of plane 54 about axis x' forms angle .theta. between axes z and z'. Finally, rotation of plane 54 about axis z' forms angle .PSI. between axis x' and the line of intersection of planes 24 and 54.
Each set of values for x, y, z, .phi., .theta., .PSI. of the reference point R and plane 54 of member 14 produces a unique set of values for the coordinates .theta..sub.1 -.theta..sub.6.
Force/position control is a current research topic in robot control. A typical method which has been proposed is as follows. A projection operator P.sub.F defines the direction in which one wants to control the force and a complementary projection operator P.sub.P defines the directions in which one wants to control position. A feedback loop is then implemented in that sensors measure the six joint angles .theta..sub.1, .theta..sub.2, .theta..sub.3, .theta..sub.4, .theta..sub.5, and .theta..sub.6, and a nonlinear transformation on these angles is performed to yield the three position coordinates x, y and z and the three Euler angles .phi., .theta., and .PSI.. Three angle corrections EQU .DELTA..phi.=.phi.-.phi..sub.R, (1a) EQU .DELTA..theta.=.theta.-.theta..sub.R ( 1b) EQU .DELTA..PSI.=.PSI.-.PSI..sub.R ( 1c)
are computed where .phi..sub.R, .theta..sub.R and .PSI..sub.R are desired or setpoint values. Three position corrections EQU .DELTA.x=x-x.sub.R ( 2a) EQU .DELTA.y=y-y.sub.R ( 2b) EQU .DELTA.z=z-z.sub.R ( 2c)
are also computed where x, y and z are actual coordinate values and x.sub.R, y.sub.R and z.sub.R denote a point on the straight line of desired motion. Then ##EQU1## defines the desired projection of the position correction. A force error ##EQU2## is also computed using the measured force vector, the desired force vector and the projection operator. PID (Proportional, Integral and Derivative) controllers are applied to the six error quantities .DELTA.f, .DELTA.u, .DELTA.v, .DELTA..phi., .DELTA..theta. and .DELTA..PSI. to produce six correction signals e.sub.1, e.sub.2, e.sub.3, e.sub.4, e.sub.5 and e.sub.6. A second nonlinear transformation is then applied to these six signals to produce correction current signals .DELTA.I.sub.1, .DELTA.I.sub.2, .DELTA.I.sub.3, .DELTA.I.sub.4, .DELTA.I.sub.5 and .DELTA.I.sub.6 to the joint motor armatures.
One of the main difficulties with the approach just described is the large amount of computation needed to evaluate the projection operators P.sub.F and P.sub.P as well as the two nonlinear transformations at each time step. The method also ignores additional dynamic coupling among the joints induced by the object, held by the robot gripper, and contacting a hard surface with a certain normal.